// $Id: camera1394.cpp 28582 2010-04-06 02:23:31Z joq $

///////////////////////////////////////////////////////////////////////////
//
// Copyright (C) 2009, 2010 Patrick Beeson, Jack O'Quin
//  ROS port of the Player 1394 camera driver.
//
// Copyright (C) 2004 Nate Koenig, Andrew Howard
//  Player driver for IEEE 1394 digital camera capture
//
// Copyright (C) 2000-2003 Damien Douxchamps, Dan Dennedy
//  Bayer filtering from libdc1394
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
// 
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
// 
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
// 02111-1307, USA.
//
///////////////////////////////////////////////////////////////////////////

#include <signal.h>
#include <ros/ros.h>
#include <boost/format.hpp>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <tf/transform_listener.h>
#include <camera_info_manager/camera_info.h>
#include <image_transport/image_transport.h>
#include <dynamic_reconfigure/server.h>

#include "camera1394.h"
#include "camera1394/Camera1394Config.h"

namespace enc = sensor_msgs::image_encodings;


/** @file

@brief camera1394 is a ROS driver for 1394 Firewire digital cameras.

This is a ROS port of the Player driver for 1394 cameras, using
libdc1394.  It provides a reliable driver with minimal dependencies,
intended to fill a role in the ROS image pipeline similar to the other
ROS camera drivers.

The ROS image pipeline provides Bayer filtering at a higher level (in
image_proc).  In some cases it is useful to run the driver without the
entire image pipeline, so libdc1394 Bayer decoding is also provided.

@par Advertises

 - \b camera/image_raw topic (sensor_msgs/Image) raw 2D camera images
   (only raw if \b bayer_method is \b NONE).

 - \b camera/camera_info topic (sensor_msgs/CameraInfo) Calibration
   information for each image.

 
@par Subscribes

 - None


@par Parameters

- \b frame_id : @b [string] camera frame of reference
     (Default: device node name)
- \b guid : @b [string] The guid of the camera (Default: "NONE")
- \b fps : @b [real] Frames per second (Default: 15.0)
- \b iso_speed : @b [int] ISO transfer speed (Default: 400)
- \b video_mode : @b [string] Desired image resolution and type (Default "800x600_mono").  The driver supports the following values:
                "320x240_yuv422"
    "640x480_mono"
    "640x480_yuv422"
    "640x480_rgb"
    "800x600_mono"
    "800x600_yuv422"
    "1024x768_mono"
    "1024x768_yuv422"
    "1280x960_mono"
    "1280x960_yuv422"
- \b bayer_pattern : @b [string] The pattern of the Bayer filter to use (Default: "NONE").  The driver supports the following values:
                "BGGR"
    "GRBG"
    "RGGB"
    "GBRG"
    "NONE"
- \b bayer_method : @b [string] The type of Bayer processing to perform (Default: "NONE").  The driver supports the following values:
                "NONE"
    "DownSample" (1/2 size image)
    "Nearest"
    "Bilinear"
    "HQ"
    "VNG"
    "AHD"
- \b exposure : @b [int] Sets the camera exposure feature to value.
- \b shutter : @b [int] Sets the camera shutter feature to value.  -1 turns on auto shutter.
- \b whitebalance : @b [string] (e.g. "2000 2000") Sets the Blue/U and Red/V components of white balance.  "auto" turns on auto white balance.
- \b gain : @b [int] Sets the camera gain feature to value.  -1 turns on auto gain.
- \b brightness : @b [int] Sets the camera brightness feature to value.


@todo Make array of supported image encoding values, check parameter
settings against that. Make enum type for dynamic reconfiguration.

*/

void sigsegv_handler(int sig);

class Camera1394Node
{
private:
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;  // TODO: verify initialization
  std::string camera_name_;
  std::string frame_id_;
  sensor_msgs::Image image_;
  sensor_msgs::Image image2_;
  sensor_msgs::CameraInfo cam_info_;
  sensor_msgs::CameraInfo cam_info2_;

  std::string camera_name_right_;
  std::string camera_name_left_;
  
  std::string camera_settings_url_;
  std::string camera2_settings_url_;
  
  CameraInfoManager *cinfo_;
  CameraInfoManager *cinfo2_;
  // initial parameters
  std::string bayerMethod_;
  std::string bayerPattern_;
  std::string encoding_;
  double frameRate_;
  std::string guid_;
  int isoSpeed_;
  std::string videoMode_;

  // reconfigurable parameters
  int brightness_;
  int exposure_;
  int gain_;
  int shutter_;
  std::string whitebalance_;

  camera1394::Camera1394Config config_;

  image_transport::CameraPublisher image_pub_;
  image_transport::CameraPublisher image2_pub_;
  
public:
  static camera1394::Camera1394* dev_;

  Camera1394Node(const ros::NodeHandle& nh): nh_(nh), it_(nh)
  {
    signal(SIGSEGV, &sigsegv_handler);
    
    getInitParams();

    // set reconfigurable parameter defaults (all to automatic)
    brightness_ = -1;
    encoding_ = "";
    exposure_ = -1;
    gain_ = -1;
    shutter_ = -1;
    whitebalance_ = "auto";

    // TODO: I really dislike opening the device in a constructor
    try
      {
        dev_ = new camera1394::Camera1394();
        if (dev_->open(guid_.c_str(), videoMode_.c_str(),
                       frameRate_, isoSpeed_, 
                       bayerPattern_.c_str(), bayerMethod_.c_str()) == 0)
          {
            ROS_INFO_STREAM("[" << camera_name_ << "] connected to device, ID: "
                            << dev_->device_id_);
          }

      }
    catch (camera1394::Exception& e)
      {
        ROS_FATAL_STREAM("[" << camera_name_ << "] exception opening device: "
                         << e.what());
        nh_.shutdown();
        return;
      }

    if (dev_->numberOfCameras() == 2) {
      image_pub_ = it_.advertiseCamera("right/image_raw", 1);
      image2_pub_ = it_.advertiseCamera("left/image_raw", 1);
      
      camera_name_right_ = camera_name_ + "/right";
      camera_name_left_ = camera_name_ + "/left";
      
      cinfo_ = new CameraInfoManager(nh_, camera_name_right_,
                                     camera_settings_url_, "right/set_camera_info");
      cinfo2_ = new CameraInfoManager(nh_, camera_name_left_,
                                      camera2_settings_url_, "left/set_camera_info");
    } else {
      image_pub_ = it_.advertiseCamera("image_raw", 1);
      cinfo_ = new CameraInfoManager(nh_, camera_name_);
    }
  } 

  ~Camera1394Node()
  {
    if (dev_)
      {
        dev_->close();
        delete dev_;
        dev_ = NULL;
      }

    delete cinfo_;
    delete cinfo2_;
  }

  /** get initial parameters (only when node starts). */
  void getInitParams(void)
  {
    if (!nh_.getParam("frame_id", frame_id_))
      {
        frame_id_ = "camera";
      }

    // resolve frame ID using tf_prefix parameter
    std::string tf_prefix = tf::getPrefixParam(nh_);
    ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
    frame_id_ = tf::resolve(tf_prefix, frame_id_);

    // default camera name is base name of frame ID
    size_t basepos = frame_id_.find_last_of("/");
    camera_name_ = frame_id_.substr(basepos+1);

    // read other parameters
    nh_.param("bayer_method", bayerMethod_, std::string("NONE"));
    nh_.param("bayer_pattern", bayerPattern_, std::string("NONE"));
    nh_.param("fps", frameRate_, 15.0);
    nh_.param("guid", guid_, std::string("NONE"));
    nh_.param("iso_speed", isoSpeed_, 400);
    nh_.param("video_mode", videoMode_, std::string("800x600_mono"));

    nh_.param("settings_url", camera_settings_url_, std::string(""));
    nh_.param("settings_url2", camera2_settings_url_, std::string(""));

    ROS_INFO_STREAM("[" << camera_name_ << "] video mode: " << videoMode_
                    << ", frame ID: " << frame_id_);
  }

  /** Update reconfigurable parameter.
   *
   * This is done every frame, so we use getParamCached() to avoid
   * unnecessary parameter server requests.
   *
   * @param name ROS parameter private name
   * @param val current parameter value; updated on exit
   * @return true if @a val changed this time
   */
  bool inline updateParam(const std::string &name, int &val)
  {
    bool changed = false;
    int prev = val;
    if (nh_.getParamCached(name, val) && (val != prev))
      changed = true;
    return changed;
  }

  /** Update reconfigurable parameter (for strings). */
  bool inline updateParam(const std::string &name, std::string &val)
  {
    bool changed = false;
    std::string prev = val;
    if (nh_.getParamCached(name, val) && (val != prev))
      changed = true;
    return changed;
  }

  /** Check for changes in reconfigurable parameters. */
  void getParameters()
  {
    static bool first_cycle = true;

    if (updateParam("brightness", brightness_) || first_cycle)
      {
        if (dev_->setBrightness(brightness_) >= 0)
          {
            if (brightness_ >= 0)
              ROS_INFO_STREAM("[" << camera_name_ << "] Brightness set to "
                              << brightness_);
            else
              ROS_INFO_STREAM("[" << camera_name_ << "] Auto Brightness set");
          }
      }

    if (updateParam("exposure", exposure_) || first_cycle)
      {
        if (dev_->setExposure(exposure_) >= 0)
          {
            if (exposure_ >= 0)
              ROS_INFO_STREAM("[" << camera_name_ << "] Exposure set to "
                              << exposure_);
            else
              ROS_INFO_STREAM("[" << camera_name_ << "] Auto Exposure set");
          }
      }
    
    if (updateParam("gain", gain_) || first_cycle)
      {
        if (dev_->setGain (gain_) >= 0)
          {
            if (gain_ >= 0)
              ROS_INFO_STREAM("[" << camera_name_ << "] Gain set to " << gain_);
            else
              ROS_INFO_STREAM("[" << camera_name_ << "] Auto Gain set");
          }
      }

    if (updateParam("shutter", shutter_) || first_cycle)
      {
        if (dev_->setShutter(shutter_) >= 0)
          {
            if (shutter_ >= 0)
              ROS_INFO_STREAM("[" << camera_name_ << "] Shutter set to "
                              << shutter_);
            else
              ROS_INFO_STREAM("[" << camera_name_ << "] Auto Shutter set");
          }
      }
    
    if (updateParam("whitebalance", whitebalance_) || first_cycle)
      {
        if (dev_->setWhiteBalance(whitebalance_.c_str()) >= 0)
          {
            if (whitebalance_ != "auto")
              ROS_INFO_STREAM("[" << camera_name_ << "] Whitebalance set to "
                              << whitebalance_);
            else
              ROS_INFO_STREAM("[" << camera_name_ << "] Auto Whitebalance set");
          }
      }


    if (updateParam("encoding", encoding_) || first_cycle)
      {
#if 0   // TODO: not sure about this...
        // set default encoding based on video mode
        size_t encpos = videoMode_.find_last_of("_");
        std::string encType = videoMode_.substr(encpos+1);
        if (encType == "mono")
          encoding_ = enc::MONO8;
        else
          encoding_ = enc::RGB8;
#endif
        ROS_INFO_STREAM("[" << camera_name_ << "] Encoding set to "
                        << encoding_);
      }

    first_cycle = false;
  }

  /** Main driver loop */
  bool spin()
  {
    while (nh_.ok())
      {
        getParameters();                // check reconfigurable parameters

        // get current CameraInfo data
        cam_info_ = cinfo_->getCameraInfo();
        image_.header.frame_id = cam_info_.header.frame_id = frame_id_;
        image2_.header.frame_id = image_.header.frame_id;

        try
          {
            // Read data from the Camera
            dev_->readData(image_, image2_);

            cam_info_.header.stamp = image_.header.stamp;
            cam_info_.height = image_.height;
            cam_info_.width = image_.width;
            if (encoding_ != "")
              image_.encoding = encoding_; // override driver setting

            // Publish it via image_transport
            image_pub_.publish(image_, cam_info_);

            //Publish the other image if requested
            if (dev_->numberOfCameras() == 2) {
              if (encoding_ != "") {
                image2_.encoding = encoding_;
              }
              
              cam_info2_ = cinfo2_->getCameraInfo();
              cam_info2_.header.frame_id = cam_info_.header.frame_id;
              cam_info2_.header.stamp = cam_info_.header.stamp;
              cam_info2_.height = cam_info_.height;
              cam_info2_.width = cam_info_.width;
              
              image2_pub_.publish(image2_, cam_info2_);
            }
          }
        catch (camera1394::Exception& e) {
          ROS_WARN_STREAM("[" << camera_name_ << "] Exception reading data: "
                          << e.what());
          //TODO: shut down and exit?
        }
      
        ros::spinOnce();
      }

    return true;
  }
};

// TODO: figure out a clean way to do this inside Camera1394Node:
camera1394::Camera1394* Camera1394Node::dev_ = NULL;

/** Segfault signal handler */
void sigsegv_handler(int sig)
{
  signal(SIGSEGV, SIG_DFL);
  fprintf(stderr, "Segmentation fault, stopping camera driver.\n");
  if (Camera1394Node::dev_)
    {
      Camera1394Node::dev_->close();
    }
}

void reconfig(camera1394::Camera1394Config &config, uint32_t level)
{
  ROS_INFO_STREAM("Reconfigure request: brightness " << config.brightness
                  << ", encoding " << config.encoding
                  << ", exposure " << config.exposure
                  << ", gain " << config.gain
                  << ", shutter " << config.shutter
                  << ", whitebalance " << config.whitebalance);

  // TODO: check parameter values
  if (config.whitebalance == "")
    config.whitebalance = "auto";

  ROS_DEBUG_STREAM("Reconfigured to: brightness " << config.brightness
                  << ", encoding " << config.encoding
                  << ", exposure " << config.exposure
                  << ", gain " << config.gain
                  << ", shutter " << config.shutter
                  << ", whitebalance " << config.whitebalance);
}


/** Main entry point */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "camera1394");

  dynamic_reconfigure::Server<camera1394::Camera1394Config> srv;
  dynamic_reconfigure::Server<camera1394::Camera1394Config>::CallbackType f =
    boost::bind(&reconfig, _1, _2);
  srv.setCallback(f);

  ros::NodeHandle nh("~");

  Camera1394Node cm(nh);

  cm.spin();
  return 0;
}

